;;;; -*- Mode: Lisp; Package: :dreaming-tree -*-
;;;;
;;;; bounding-volumes.lisp
;;;;

(in-package :dreaming-tree)

;;;; === Bounding volumes ===

;; The API

(defgeneric compute-bound-from-data (bound vertex-data))
(defgeneric transform-bound-by (bound transformation))
(defgeneric copy-bound (bound))
(defgeneric volume (bound)
  (:documentation "Calculate the volume of the bounding-volume."))
(defgeneric grow-to-contain (bound input-bound)
  (:documentation "Return a new bounding volume that contains both bound and input-bound."))
(defgeneric contains-p (bound point))
(defgeneric center (bound))
(defgeneric radius (bound))

;;; Bounding sphere

(defstruct (bsphere (:conc-name bs-))
  (c (zero-vector 3) :type vector)
  (r 1.0 :type single-float))

(defmethod center ((b bsphere))
  (bs-c b))

(defmethod radius ((b bsphere))
  (bs-r b))

;;; Axis-aligned bounding boxes (AABB)

(defstruct aabb
  (c (zero-vector 3) :type vector)
  (e (vector 1.0 1.0 1.0) :type vector))

(defun aabb-min (aabb)
  (m- (aabb-c aabb) (aabb-e aabb)))

(defun aabb-max (aabb)
  (m+ (aabb-c aabb) (aabb-e aabb)))

;;; Oriented bounding box.

(defstruct oobb
  (u (identity-matrix 3) :type array)
  (c (zero-vector 3) :type vector)
  (e (vector 1.0 1.0 1.0) :type vector))

(defmethod copy-bound ((b bsphere))
  (make-bsphere :c (bs-c b) :r (bs-r b)))

(defmethod on-move ((b bsphere) new-pos)
  (setf (bs-c b) new-pos))

(defmethod transform-bound-by ((b bsphere) (m transform))
  (make-bsphere :c (apply-forward m (bs-c b))
		:r (* (norm m) (bs-r b))))

(defmethod grow-to-contain (bound input-bound)
  ;; Doesn't seem right - still need to understand semantics of composing bounding volumes.
  (make-bsphere :c (m* 0.5 (m+ (center bound) (center input-bound)))
		:r (+ (radius bound) (radius input-bound))))

(defun make-bound (&key (center (zero-vector 3)))
  (make-oobb :c center))

(defmethod radius ((s spatial))
  (radius (bounding-volume s)))

(defmethod radius ((obb oobb))
  (magnitude (m* (oobb-u obb) (oobb-e obb))))

(defmethod bounding-vertices ((b oobb))
  (let* ((c (oobb-c b))
	 (u (oobb-u b)) 
	 (e (oobb-e b))
	 (x (m* (get-column u 0) (mref e 0)))
	 (y (m* (get-column u 1) (mref e 1)))
	 (z (m* (get-column u 2) (mref e 2))))
    (list (m+ c       x        y        z)
	  (m+ c       x        y  (mneg z))
	  (m+ c       x  (mneg y)       z)
	  (m+ c       x  (mneg y) (mneg z))
	  (m+ c (mneg x)       y        z)
	  (m+ c (mneg x)       y  (mneg z))
	  (m+ c (mneg x) (mneg y)       z)
	  (m+ c (mneg x) (mneg y) (mneg z)))))

(defmethod volume ((b oobb))
  (let ((e (oobb-e b)))
    (* (mref e 0) (mref e 1) (mref e 2))))

(defmethod set-axis ((obb oobb) new-axis n)
  (set-column (oobb-u obb) n new-axis))

(defmethod get-axis ((obb oobb) n)
  (get-column (oobb-u obb) n))

(defmethod compute-bound-from-data ((bv (eql 'bsphere)) vertices)
  (sphere-centered-at-point-average vertices))

(defmethod compute-bound-from-data ((bv (eql 'oobb)) vertices)
  (box-from-mean-covariance vertices))

(defmethod grow-to-contain ((s1 spatial) (s2 spatial))
  (setf (bounding-volume s1)
	(grow-to-contain (bounding-volume s1) (bounding-volume s2))))

(defmethod grow-to-contain ((s1 spatial) (l2 list))
  (dolist (s2 l2)
    (grow-to-contain s1 s2)))

(defmethod grow-to-contain ((bound bsphere) (b2 bsphere))
  (let* ((dcenter (m- (bs-c bound) (bs-c b2)))
	 (dradius (- (bs-r bound) (bs-r b2)))
	 (drsqr (* dradius dradius))
	 (lsqr (squared-magnitude dcenter)))
    (if (>= drsqr lsqr)
	(if (> dradius 0.0)
	    b2
	    bound)
	(let* ((l (sqrt lsqr))
	       (n (/ (+ l dradius) (* 2 l))))
	(make-bsphere :c (m+ (bs-c bound) (* n dcenter))
		      :r (/ (+ l (bs-r bound) (bs-r b2)) 2))))))

(defmethod grow-to-contain ((b0 oobb) (b1 oobb))
  (let* ((box (make-oobb :c (m/ (m+ (oobb-c b0) (oobb-c b1)) 2.0)))
	 (q0 (lib.gl::quat-from-rotation-matrix (oobb-u b0)))
	 (q1 (lib.gl::quat-from-rotation-matrix (oobb-u b1)))
	 (q (zero-vector 4))
	 (min (zero-vector 3))
	 (max (zero-vector 3)))
    (if (< (dot-product q0 q1) 0.0)
	(setf q1 (mneg q1)))
    (setf q (m+ q0 q1)
	  (oobb-u box) (quat-to-rotation-matrix (m/ q (quat-magnitude q))))
    ;; Compute Projections
    (iter (for v in (bounding-vertices b0))
	  (let ((diff (m- v (oobb-c box))))
	    (iter (for j below 3)
		  (cond ((< (mref diff j) (mref min j))
			 (setf (mref min j) (mref diff j)))
			((> (mref diff j) (mref max j))
			 (setf (mref max j) (mref diff j)))))))
    (iter (for v in (bounding-vertices b1))
	  (let ((diff (m- v (oobb-c box))))
	    (iter (for j below 3)
		  (cond ((< (mref diff j) (mref min j))
			 (setf (mref min j) (mref diff j)))
			((> (mref diff j) (mref max j))
			 (setf (mref max j) (mref diff j)))))))
    ;; Adjust center and compute extents
    (iter (for j from 0 below 3)
	  (setf (oobb-c box) (m+ (oobb-c box) (m* (* 0.5 (+ (mref min j) (mref max j)))
						  (get-column (axes box) j))))
	  (setf (mref (oob-e box) j) (* 0.5 (- (mref max j) (mref min j)))))
    box))

    
    
;;;; Sphere Containing Points

(defun unpack-array-to-lists (arr element-stride)
  (iter (for i from 0 below (element-count arr) by element-stride)
	(collect (apply #'make-vector (iter (for j from 0 below element-stride)
					    (collect (row-major-aref arr (+ i j))))))))

(defun sphere-containing-axis-aligned-box (points &optional (packed-array-p t))
  ""
  (when packed-array-p
    (setf points (unpack-array-to-lists points 3)))
  (let* ((vmin (copy-vector (first points)))
	 (vmax (copy-vector (first points))))
    (iter (for v in (rest points))
	  (if (> (x v) (x vmax)) (setf (x vmax) (x v)))
	  (if (> (y v) (y vmax)) (setf (y vmax) (y v)))
	  (if (> (z v) (z vmax)) (setf (z vmax) (z v)))
	  (if (< (x v) (x vmin)) (setf (x vmin) (x v)))
	  (if (< (y v) (y vmin)) (setf (y vmin) (y v)))
	  (if (< (z v) (z vmin)) (setf (z vmin) (z v))))
    (make-bsphere :c (m/ (m+ vmax vmin) 2)
		  :r (magnitude (m/ (m- vmax vmin) 2)))))

(defun sphere-centered-at-point-average (points &optional (packed-array-p t))
  (when packed-array-p
    (setf points (unpack-array-to-lists points 3)))
  (let ((sum (zero-vector 3)))
    (iter (for v in points)
	  (setf sum (m+ sum v)))
    (let ((center (m/ sum (length points)))
	  (r2 0.0))
      (iter (for v in points)
	    (let ((temp (squared-magnitude (m- v center))))
	      (if (> temp r2)
		  (setq r2 temp))))
      (make-instance 'bounding-sphere
		     :center center
		     :radius (sqrt r2)))))
      
  

;;; Box Containing Points

(defun box-from-mean-covariance (points &optional (packed-array-p t))
  (when packed-array-p
    (setf points (unpack-array-to-lists points 3)))
  (let* ((sum (zero-vector 3))
	 (mean (m/ (iter (for p in points)
			 (setf sum (m+ sum p))
			 (finally (return sum)))
		   (length points)))
	 (box (make-oobb :c mean))	 
	 (mat (zero-matrix 3)) ;; Holds the covariance matrix
	 (min (zero-vector 3))
	 (max (zero-vector 3))
	 (diff (zero-vector 3)))
    (with-slots (c u e) box
      (iter (for p in points)
	    (setq diff (m- p c))
	    (setf mat (m+ mat (make-tensor-product diff diff))))
      (iter (with m = (m/ mat (length points)))
	    (for a in (find-eigenvectors m (find-eigenvalues m)))
	    (for i from 0 by 1)
	    (set-column u i a))
      ;; Compute extremes of points projected onto the box axes.
      (setq diff (m- (first points) c))
      (iter (for j from 0 below 3)
	    (setf (mref min j) (dot-product (get-column u j) diff)
		  (mref max j) (mref min j)))
      (iter (for p in points)
	    (setq diff (m- p c))
	    (iter (for j from 0 below 3)
		  (for tmp = (dot-product (get-column u j) diff))
		  (cond ((< tmp (mref min j)) (setf (mref min j) tmp))
			((> tmp (mref max j)) (setf (mref max j) tmp)))))
      ;; Adjust the box's center to be the average of the extremens (min and max)
      ;; and compute the extents.
      (iter (for j from 0 below 3)
	    (setf c (m+ c (m* (* 0.5 (+ (mref min j) (mref max j))) (get-column u j))))
	    (setf (mref e j) (* 0.5 (- (mref max j) (mref min j)))))
      box)))
    


;;; === Testing ===
(defun test-bs-compute ()
  (let* ((vn (list (make-vector 1.0 1.0 1.0)
		   (make-vector -1.0 1.0 1.0)
		   (make-vector -1.0 -1.0 1.0)
		   (make-vector 1.0 -1.0 1.0)
		   (make-vector 1.0 -1.0 -1.0)
		   (make-vector 1.0 1.0 -1.0)
		   (make-vector -1.0 1.0 -1.0)
		   (make-vector -1.0 -1.0 -1.0))))
    (format t "~A~%" (sphere-containing-axis-aligned-box vn nil))
    (format t "~A~%" (sphere-centered-at-point-average vn nil))
    (format t "~A~%" (box-from-mean-covariance vn nil))))

(defun test-grow ()
  (let ((b0 (make-oobb :c #(0.0 0.0 0.0) :e #(1.0 1.0 1.0)))
	(b1 (make-oobb :c #(0.0 0.0 0.0) :e #(3.0 3.0 3.0))))
    (format t "~A~%" (bounding-vertices b0))
    (format t "~A~%" (bounding-vertices b1))
    (grow-to-contain b0 b1)))

(defun test-bounding-box ()
  (let ((box (make-oobb :c #(0.0 0.0 0.0) :e #(1.0 1.0 1.0))))
    (format t "Radius=~A" (radius box))))
